#include"speech_server/SpeechServer.hpp"
#include<std_msgs/Float64.h>
#include<iostream>
#include<QApplication>
SpeechServerNode::SpeechServerNode(ComminucationHandler *handler):WorkingThread("SpeechServer",handler) /*:communication()*/
{

}

SpeechServerNode::~SpeechServerNode()
{
    delete get_Speech_Action;
    delete get_Speech_Ignore_Action;
    delete get_Speech_Just_Action;
    delete get_Speech_yesno_Action;
    delete nh;

}
void SpeechServerNode::stop()
{
    qDebug()<<"void SpeechServerNode::stop()";
    running=false;
    usleep(100);
    ros::shutdown();
//    communication.close();
    quit();
    wait();
}

void SpeechServerNode::run()
{
    running=true;
    ros::Rate loop_rate(10);
    while(!ros::isShuttingDown() && (running))
    {
        ros::spinOnce();
        loop_rate.sleep();
    }
    qDebug()<<"nodeFinished";
    Q_EMIT rosShutdown();
    exec();

}
bool SpeechServerNode::init(int argc, char *argv[])
{
    try
    {

        ros::init(argc,argv,"SpeechServer");
        ros::start();
        nh=new ros::NodeHandle("~");
                position_publisher=nh->advertise<std_msgs::Float64>("//dynamixel_controller/head_pan_controller/command",1);
        //Initializing Network
        ShowInfoAndRosInfo("Initializing Network");
        //int port;
        //string host;
        //nh->param<int>("port",port,SPEECHSERVER_PORT);
        //nh->param<string>("host",host,"192.168.10.112");
        //Initializing Services
        ShowInfoAndRosInfo("Initializing Services");
        LoadGrammarService      = nh->advertiseService("SpeechLoadGrammar" ,
                                                       &SpeechServerNode::loadGrammarCB ,this);
        UnLoadGrammarService    = nh->advertiseService("SpeechUnLoadGrammar" ,
                                                       &SpeechServerNode::unLoadGrammarCB ,this);
        UnLoadAllGrammarService = nh->advertiseService("SpeechUnLoadAllGrammar" ,
                                                       &SpeechServerNode::unLoadAllGrammarCB,this);
        SetRecognitionConfidenceService =nh->advertiseService("SetRecognitionConfidence" ,
                                                              &SpeechServerNode::setRecognitionConfidenceCB ,this);
        SayService              = nh->advertiseService("Say" ,&SpeechServerNode::sayCB ,this);

        //Initializing Actions
        ShowInfoAndRosInfo("Initializing");
        get_Speech_Action = new GetSpeechAction(*nh,
                                                "/GetSpeech",
                                                boost::bind(&SpeechServerNode::GetSpeechActionExecuteCB,this, _1),
                                                false );
        if(!get_Speech_Action)
            return false;
        get_Speech_Action->start();
        get_Speech_Just_Action  = new GetSpeechJustAction(*nh,
                                                          "/GetSpeech_Just" ,
                                                          boost::bind(&SpeechServerNode::GetSpeech_JustActionExecuteCB , this, _1) ,
                                                          false );
        if(!get_Speech_Just_Action)
            return false;
        get_Speech_Just_Action->start();
        get_Speech_Ignore_Action= new GetSpeechIgnoreAction(*nh,
                                                            "/GetSpeech_Ignore" ,
                                                            boost::bind(&SpeechServerNode::GetSpeech_IgnoreActionExecuteCB , this, _1),
                                                            false );
        if(!get_Speech_Ignore_Action)
            return false;
        get_Speech_Ignore_Action->start();
        get_Speech_yesno_Action = new GetSpeechYesNoAction(*nh,
                                                           "/GetSpeech_YesNo" ,
                                                           boost::bind(&SpeechServerNode::GetSpeech_yesnoActionExecuteCB ,this ,_1),
                                                           false );
        if(!get_Speech_yesno_Action)
            return false;
        get_Speech_yesno_Action->start();
    }
    catch(...)
    {
        return false;
    }
    return true;
}

//Recogniton functions
//Result SpeechServerNode::GetSpeech(string category)
//{
//    ShowInfo("GetSpeech Action has been called");

//    Command c;
//    c.commandtype="GetSpeech";
//    c.args.push_back(Arg("mode","simple"));
//    c.args.push_back(Arg("Category",category));

//    communication.Send(XmlProccessor::Generate(c));
//    return XmlProccessor::Parse(communication.Recieve());
//}

//Result SpeechServerNode::GetSpeech_Ignore(vector<string> commands, string category)
//{
//    ShowInfo("GetSpeech Ignore Action has been called");
//    Command c;
//    c.commandtype="GetSpeech";
//    c.args.push_back(Arg("mode","ignore"));
//    c.args.push_back(Arg("Category",category));
//    if(commands.size()>0)
//    {
//        string cmds="";
//        for(unsigned int i=0;i<commands.size();i++)
//        {
//            cmds+=commands[i]+",";
//        }
//        cmds.erase(cmds.size()-1,cmds.size()-1);
//        c.args.push_back(Arg("Cmds",cmds));
//    }
//    communication.Send(XmlProccessor::Generate(c));
//    return XmlProccessor::Parse(communication.Recieve());
//}

//Result SpeechServerNode::GetSpeech_Just(vector<string> commands,std::string category)
//{
//    ShowInfo("GetSpeech Just Action has been called");
//    Command c;

//    c.commandtype="GetSpeech";
//    c.args.push_back(Arg("mode","just"));
//    c.args.push_back(Arg("Category",category));
//    string cmds="";
//    if(commands.size())
//    {
//        for(unsigned int i=0;i<commands.size();i++)
//        {
//            cmds+=commands[i]+",";
//        }
//        cmds.erase(cmds.size()-1,cmds.size()-1);
//        c.args.push_back(Arg("Cmds",cmds));
//    }

//    communication.Send(XmlProccessor::Generate(c));
//    return XmlProccessor::Parse(communication.Recieve());

//}
//Result SpeechServerNode::GetSpeech_yesno()
//{
//    ShowInfo("GetSpeech Just Action has been called");
//    Command c;
//    SpeechServer::GetSpeech_yesnoFeedback feedback;
//    c.commandtype="GetSpeech";
//    c.args.push_back(Arg("mode","yesno"));
//    communication.Send(XmlProccessor::Generate(c));
//    return XmlProccessor::Parse(communication.Recieve());
//}

//End of Recogniton functions


//Actions Callback:
void SpeechServerNode::GetSpeechActionExecuteCB(const speech_server::GetSpeechGoalConstPtr& goal)
{
    static int non_recognized_name_count=0;
    static string non_recognized_name[]={"person_A","Person_B","Person_C"};

    Command c;
    speech_server::GetSpeechFeedback feedback;
    sensor_msgs::JointState headState;
    ShowInfoAndRosInfo("GetSpeech Action has been called");
    c.commandtype="GetSpeech";
    c.args.push_back(Arg("mode","simple"));
    c.args.push_back(Arg("Category",goal->Category.c_str()));

    Result result;
    Result yesno;
    int tries=0;
    do
    {
        if(tries>3 && goal->Category=="name")
        {
            ShowInfoAndRosInfo("assuming the name is "+non_recognized_name[non_recognized_name_count]);
            SayText("I assumed that your name is "+non_recognized_name[non_recognized_name_count]);
            result.command=non_recognized_name[non_recognized_name_count];
            result.confidence=1;
            result.success=1;

            non_recognized_name_count++;
            non_recognized_name_count%=3;
            break;
        }

        ShowInfoAndRosInfo("Recognizing");
        result=XmlProccessor::Parse(commitCommand(getWorkerName(),XmlProccessor::Generate(c)));
        ShowInfoAndRosInfo("Command=\t "+result.command);
        ShowInfoAndRosInfo("Success=\t"+result.success);
        ShowInfoAndRosInfo("Text=\t"+result.text);
        if(!result.success)
        {
            ShowInfoAndRosInfo("Rejected");
            feedback.result=SpeechResultToRosSpeechResult(result);
            get_Speech_Action->publishFeedback(feedback);
            continue;
        }
        if(result.command=="look")
        publishJointState(result.position);
        if(goal->verify)
        {
            do
            {
                ShowInfoAndRosInfo("Verifying");
                SayText(string("Did you said ")+result.text);
                Command yesNoCommand;
                yesNoCommand.commandtype="GetSpeech";
                yesNoCommand.args.push_back(Arg("mode","yesno"));
                ShowInfoAndRosInfo("Yes No Recognizing");
                yesno=XmlProccessor::Parse(commitCommand(getWorkerName(),XmlProccessor::Generate(yesNoCommand)));
                if(yesno.success)
                {
//                    publishJointState(yesno.position);
                    if(yesno.command=="yes")
                        break;
                    else
                    {
                        ShowInfoAndRosInfo("Repeating recognition");
                        SayText(string("Please repeat your command again"));
                        result.success=false;
                        break;
                    }
                }
                else
                {
                    ShowInfoAndRosInfo("FeedBacking");
                    feedback.result=SpeechResultToRosSpeechResult(yesno);
                    get_Speech_Action->publishFeedback(feedback);
                    continue;
                }
            }while(yesno.success);
        }
        tries++;
    }  while(!result.success);
    speech_server::GetSpeechResult action_result;
    action_result.result=SpeechResultToRosSpeechResult(result);
    get_Speech_Action->setSucceeded(action_result);
}

void SpeechServerNode::GetSpeech_IgnoreActionExecuteCB(const speech_server::GetSpeech_IgnoreGoalConstPtr& goal)
{
    ShowInfoAndRosInfo("GetSpeech Ignore Action has been called");
    Command c;
    speech_server::GetSpeech_IgnoreFeedback feedback;
    c.commandtype="GetSpeech";
    c.args.push_back(Arg("mode","ignore"));
    c.args.push_back(Arg("Category",goal->Category));
    if(goal->Cmds.size())
    {
        string cmds="";
        for(unsigned int i=0;i<goal->Cmds.size();i++)
        {
            cmds+=goal->Cmds[i]+",";
        }
        cmds.erase(cmds.size()-1,cmds.size()-1);
        c.args.push_back(Arg("Cmds",cmds));
    }
    Result result;
    Result yesno;
    do
    {
        ShowInfoAndRosInfo("Recognizing");

//        communication.Send(XmlProccessor::Generate(c));
//        result=XmlProccessor::Parse(communication.Recieve());
                result=XmlProccessor::Parse(commitCommand(getWorkerName(),XmlProccessor::Generate(c)));
        ShowInfoAndRosInfo("Command=\t "+result.command);
        //ShowInfoAndRosInfo("Success=\t "+result.success);
        ShowInfoAndRosInfo("Text=\t"+result.text);
        if(!result.success)
        {
            ShowInfoAndRosInfo("Rejected");
            ShowInfoAndRosInfo("FeedBacking");
            feedback.result=SpeechResultToRosSpeechResult(result);
            get_Speech_Ignore_Action->publishFeedback(feedback);
            continue;
        }
        if(result.command=="look")
        publishJointState(result.position);
        if(goal->verify)
        {
            do
            {
                ShowInfoAndRosInfo("Verifying");
                SayText(string("Did you said ")+result.text);
                Command yesNoCommand;
                yesNoCommand.commandtype="GetSpeech";
                yesNoCommand.args.push_back(Arg("mode","yesno"));
                ShowInfoAndRosInfo("Yes No Recognizing");
//                communication.Send(XmlProccessor::Generate(yesNoCommand));
//                yesno=XmlProccessor::Parse(communication.Recieve());
                yesno=XmlProccessor::Parse(commitCommand(getWorkerName(),XmlProccessor::Generate(yesNoCommand)));
                if(yesno.success)
                {
//                    publishJointState(yesno.position);
                    if(yesno.command=="yes")
                        break;
                    else
                    {
                        ShowInfoAndRosInfo("Repeating recognition");
                        SayText(string("Please repeat your command again"));
                        result.success=false;
                        break;
                    }
                }
                else
                {
                    ShowInfoAndRosInfo("FeedBacking");
                    feedback.result=SpeechResultToRosSpeechResult(yesno);
                    get_Speech_Ignore_Action->publishFeedback(feedback);
                    continue;
                }
            }while(yesno.success);
        }
    }  while(!result.success);
    speech_server::GetSpeech_IgnoreResult action_result;
    action_result.result=SpeechResultToRosSpeechResult(result);
    get_Speech_Ignore_Action->setSucceeded(action_result);
}

void SpeechServerNode::GetSpeech_JustActionExecuteCB(const speech_server::GetSpeech_JustGoalConstPtr& goal)
{
    ShowInfoAndRosInfo("GetSpeech Just Action has been called");
    Command c;
    speech_server::GetSpeech_JustFeedback feedback;
    c.commandtype="GetSpeech";
    c.args.push_back(Arg("mode","just"));
    c.args.push_back(Arg("Category",goal->Category.c_str()));
    string cmds="";
    if(goal->Cmds.size())
    {
        for(unsigned int i=0;i<goal->Cmds.size();i++)
        {
            cmds+=goal->Cmds[i]+",";
        }
        cmds.erase(cmds.size()-1,cmds.size()-1);
        c.args.push_back(Arg("Cmds",cmds));
    }
    Result result;
    Result yesno;
    do
    {
        ShowInfoAndRosInfo("Recognizing");
//        communication.Send(XmlProccessor::Generate(c));
//        result=XmlProccessor::Parse(communication.Recieve());

        result=XmlProccessor::Parse(commitCommand(getWorkerName(),XmlProccessor::Generate(c)));
        ShowInfoAndRosInfo("Command=\t"+result.command);
        //ShowInfoAndRosInfo("Success=\t"+result.success);
        ShowInfoAndRosInfo("Text=\t"+result.text);
        if(!result.success)
        {
            ShowInfoAndRosInfo("Rejected");
            ShowInfoAndRosInfo("FeedBacking");
            feedback.result=SpeechResultToRosSpeechResult(result);
            get_Speech_Just_Action->publishFeedback(feedback);
            continue;
        }
        if(result.command=="look")
            publishJointState(result.position);
        if(goal->verify)
        {
            do
            {
                ShowInfoAndRosInfo("Verifying");
                SayText(string("Did you said ")+result.text);
                Command yesNoCommand;
                yesNoCommand.commandtype="GetSpeech";
                yesNoCommand.args.push_back(Arg("mode","yesno"));
                ShowInfoAndRosInfo("Yes No Recognizing");

//                communication.Send(XmlProccessor::Generate(yesNoCommand));
//                yesno=XmlProccessor::Parse(communication.Recieve());

                yesno=XmlProccessor::Parse(commitCommand(getWorkerName(),XmlProccessor::Generate(yesNoCommand)));

                if(yesno.success)
                {
//                    publishJointState(yesno.position);
                    if(yesno.command=="yes")
                        break;
                    else
                    {
                        ShowInfoAndRosInfo("Repeating recognition");
                        SayText(string("Please repeat your command again"));
                        result.success=false;
                        break;
                    }
                }
                else
                {
                    ShowInfoAndRosInfo("FeedBacking");
                    feedback.result=SpeechResultToRosSpeechResult(yesno);
                    get_Speech_Just_Action->publishFeedback(feedback);
                    continue;
                }
            }while(yesno.success);
        }
    }  while(!result.success);
    speech_server::GetSpeech_JustResult action_result;
    action_result.result=SpeechResultToRosSpeechResult(result);
    get_Speech_Just_Action->setSucceeded(action_result);
}

void SpeechServerNode::GetSpeech_yesnoActionExecuteCB(const speech_server::GetSpeech_yesnoGoalConstPtr& goal)
{
    ShowInfoAndRosInfo("GetSpeech Just Action has been called");
    Command c;
    speech_server::GetSpeech_yesnoFeedback feedback;
    c.commandtype="GetSpeech";
    c.args.push_back(Arg("mode","yesno"));
    Result result;
    do
    {
        ShowInfoAndRosInfo("Recognizing");
//        communication.Send(XmlProccessor::Generate(c));
//        result=XmlProccessor::Parse(communication.Recieve());
                result=XmlProccessor::Parse(commitCommand(getWorkerName(),XmlProccessor::Generate(c)));

        ShowInfoAndRosInfo("Command=\t"+result.command);
        //ShowInfoAndRosInfo("Success=\t"+result.success);
        ShowInfoAndRosInfo("Text=\t"+result.text);
        if(!result.success)
        {
            ShowInfoAndRosInfo("Rejected");
            ShowInfoAndRosInfo("FeedBacking");
            feedback.result=SpeechResultToRosSpeechResult(result);
            get_Speech_yesno_Action->publishFeedback(feedback);
            continue;
        }
    }  while(!result.success);
//    publishJointState(result.position);
    speech_server::GetSpeech_yesnoResult action_result;
    action_result.result=SpeechResultToRosSpeechResult(result);
    get_Speech_yesno_Action->setSucceeded(action_result);
}

//End of Action Callback
//Command functions
bool SpeechServerNode::loadGrammar(std::string grammarname)
{
    Command c;
    c.commandtype="LoadGrammar";
    c.args.push_back(Arg("grammar",grammarname));
    ShowInfo("Loading  Grammar"+ grammarname);
//    communication.Send(XmlProccessor::Generate(c));
//    Result r=XmlProccessor::Parse(communication.Recieve());
    Result r=XmlProccessor::Parse(commitCommand(getWorkerName(),XmlProccessor::Generate(c)));
    return r.success;
}

bool SpeechServerNode::unLoadGrammar(std::string grammarname)
{
    Command c;
    c.commandtype="UnLoadGrammar";
    c.args.push_back(Arg("grammar",grammarname));
    ShowInfo ("unLoading  Grammar"+ grammarname);
//    communication.Send(XmlProccessor::Generate(c));
//    Result r=XmlProccessor::Parse(communication.Recieve());
    Result r=XmlProccessor::Parse(commitCommand(getWorkerName(),XmlProccessor::Generate(c)));
    return r.success;
}

bool SpeechServerNode::unLoadAllGrammar()
{
    Command c;
    c.commandtype="UnLoadAllGrammars";
    ShowInfo("UnLoading All Grammars");
//    communication.Send(XmlProccessor::Generate(c));
//    Result r=XmlProccessor::Parse(communication.Recieve());
    Result r=XmlProccessor::Parse(commitCommand(getWorkerName(),XmlProccessor::Generate(c)));
    return r.success;
}

bool SpeechServerNode::setRecognitionConfidence(int confidence)
{
    qDebug("notImplemented yet");
    return false;
//    Command c;
//    c.commandtype="SetConfidence";
//    ShowInfo("Setting Confidence to %d Grammar" + QString::number(confidence).toStdString());
//    stringstream str;
//    str<<confidence;
//    c.args.push_back(Arg("confidence",str.str()));
//    return XmlProccessor::Parse(communication.Recieve()).success;
}

bool SpeechServerNode::say(std::string quatation)
{
    return SayText(quatation);
}

//End of Command functions
//Services Callbacks:
bool SpeechServerNode::loadGrammarCB(speech_server::LoadGrammar::Request& req,speech_server::LoadGrammar::Response& res)
{
    ShowInfoAndRosInfo("LoadGrammar Service is running");
    return loadGrammar(req.grammarname);
}

bool SpeechServerNode::unLoadGrammarCB(speech_server::UnLoadGrammar::Request& req,speech_server::UnLoadGrammar::Response& res)
{
    ShowInfoAndRosInfo("UnLoadGrammar Service is running");
    return unLoadGrammar(req.grammarname);
}

bool SpeechServerNode::unLoadAllGrammarCB(speech_server::UnLoadAllGrammar::Request& req,speech_server::UnLoadAllGrammar::Response& res)
{
    ShowInfoAndRosInfo("UnloadAllGrammars service is running");
    return unLoadAllGrammar();
}
bool SpeechServerNode::setRecognitionConfidenceCB(speech_server::SetRecognitionConfidence::Request &req, speech_server::SetRecognitionConfidence::Response &res)
{
    ShowInfoAndRosInfo("SetRecognitionConfidence Service is running");
    return setRecognitionConfidence(req.Confidence.data);
}

bool SpeechServerNode::sayCB(speech_server::Say::Request& req,speech_server::Say::Response& res)
{
    ShowInfoAndRosInfo("Say Service is running");
    return say(req.text);
}

//End of Services Callbacks

//Recognition Functions
bool SpeechServerNode::VerifyText(std::string str)
{
    SayText(str);
    Command c;
    c.commandtype="GetSpeech_yesno";
    //communication.Send(XmlProccessor::Generate(c));
    return (XmlProccessor::Parse(commitCommand(getWorkerName(),XmlProccessor::Generate(c))).command=="yes");
}
//End of Recognition Functions

//tts functions
bool SpeechServerNode::SayText(std::string str)
{
    stringstream str_stream;
    //str_stream<<"echo \""<<str<<"\"| espeak --stdin -vf5 -s150 -p70";
    str_stream<<"echo \""<<str<<"\"| espeak -v mb-us1 -p65 -s120 --stdin --stdout|aplay";
//    str_stream<<"echo \""<<str<<"\"| festival --tts";
    if(system(str_stream.str().c_str())==0)
    {

        ShowInfo(string("Said ") + str);
        return true;
    }
    else
    {
        ShowInfo("Festival Failed");
        return false;
    }
}
bool SpeechServerNode::SayText(char *str)
{
    return SayText(string(str));
}
//End of tts Functions

void SpeechServerNode::ShowInfo(std::string info)
{
    Q_EMIT Info(QString("[Test Info]: ")+info.c_str());
}
void SpeechServerNode::ShowInfoAndRosInfo(std::string info)
{
    ROS_INFO("%s",(info.c_str()));
    Q_EMIT Info(QString("[ROS INFO]: ")+info.c_str());
}

//Converting Functions
speech_server::SpeechRecognitionResult SpeechServerNode::SpeechResultToRosSpeechResult(Result res)
{
    speech_server::SpeechRecognitionResult result;
    result.Success.data=res.success;
    result.Command.data=res.command;
    result.Text.data=res.text;
    result.Confidence.data=res.confidence;
    result.Position.data=res.position;
    result.PositionConfidence.data=res.position_confidence;
    return result;
}
//End of Converting Functions
void SpeechServerNode::publishJointState(float pos)
{
//    pos=pos*(3.14)/180;
    pos=pos*(3.14)/180;
     std_msgs::Float64 msg;
     msg.data=pos;
     position_publisher.publish(msg);
}
